import argparse
import time
import logging
from math import sqrt

from bins.PlannerLogger import PlannerLogger
from src.MultiLoop_Iteration import MotionPlanner
from src.motionplanner.Constrained_ILQR.scripts.arguments import add_arguments
from tools.drawAnimation import Animation


timestamp_str = time.strftime("%Y-%m-%d_%H_%M_%S", time.localtime())
log_file = "./log/" + timestamp_str + '.log'

record = PlannerLogger()
record.load_file("./record/" + timestamp_str + ".json")


# if __name__ == "__main__":
#     argparser = argparse.ArgumentParser(description='CARLA CILQR')
#     add_arguments(argparser)
#     args = argparser.parse_args()
#     t = MotionPlanner(args)
#     # t.load_all_trajectory("../trajectory/0_100/")
#     for i in range(0, 10):
#         name = "Car" + str(i)
#         name_ = "Car" + str(i - 1)
#         t.load_car(name=name, ref_line=3.4 / 2, v= 70 / 3.6, x=46 - i * 4, y=1.7, v_=0, h=0)
#         if i != 0:
#             t.set_car_restrain(car1_name=name, car2_name=name_, distance=10)
#
#     for i in range(400):
#         # if i == 1:
#         #     t.set_car_restrain(car1_name="Car0", car2_name="Car1", distance=10)
#         print(i)
#         t.run_loop()
#
#         # t.save_all_trajectory("../trajectory/test")
#     # t.save_run_trajectory("../trajectory/0_100/")
#     # t.save_all_trajectory("../trajectory/100alldata/")
#     ani = Animation()
#     for i in t.multi_car.keys():
#
#         ani.load_trajectory(t.multi_car[i].historical_track)
#         print(t.multi_car[i].historical_track)
#
#     ani.draw()

if __name__ == "__main__":
    argparser = argparse.ArgumentParser(description='CARLA CILQR')
    add_arguments(argparser)
    args = argparser.parse_args()
    t = MotionPlanner(args)
    distan = sqrt(3.4 * 3.4 + 6.4 * 6.4)
    distance2 = 3 + 1.4
    t.load_car(name="Car0", ref_line=3.4 / 2, v=round(25 / 3.6, 2), x=0, y=3.4 / 2, v_=round(0, 2), h=0)
    t.load_car(name="Car1", ref_line=3.4 / 2 + 3, v=round(25 / 3.6, 2), x=0, y=3.4 / 2 + 3, v_=round(0, 2), h=0)


    # t.load_car(name="Car2", ref_line=3.4 / 2, v=round(40 / 3.6, 2), x=46 - 2 * distance2, y=3.4 / 2, v_=round(40 / 3.6, 2), h=0)
    # t.load_car(name="Car3", ref_line=3.4 / 2 + 3.4, v=round(40 / 3.6, 2), x=46 - 2 * distance2 - 5, y=3.4 / 2 + 3.4, v_=round(40 / 3.6, 2), h=0)
    # t.load_car(name="Car4", ref_line=3.4 / 2 + 3.4, v=round(40 / 3.6, 2), x=46 - 3 * distance2 - 5, y=3.4 / 2 + 3.4, v_=round(40 / 3.6, 2), h=0)
    # t.load_car(name="Car5", ref_line=3.4 / 2 + 3.4, v=round(40 / 3.6, 2), x=46 - 4 * distance2 - 5, y=3.4 / 2 + 3.4, v_=round(40 / 3.6, 2), h=0)
    # t.set_car_restrain("Car1", "Car0", distance2)
    # t.set_car_restrain("Car2", "Car1", distance2)

    for i in range(50):
        print(i)
        t.run_loop()

    t.update_car_state("Car1", speed=round(35/3.6, 2), ref_line=3.4 / 2)
    #
    # t.set_car_restrain("Car4", "Car3", 20)
    # t.set_car_restrain("Car5", "Car4", 20)
    # t.update_car_state("Car3", speed=round(80 / 3.6, 2), ref_line=3.4 / 2)
    # t.update_car_state("Car4", speed=round(80 / 3.6, 2), ref_line=3.4 / 2)
    # t.update_car_state("Car5", speed=round(80 / 3.6, 2), ref_line=3.4 / 2)
    # t.load_car(name="Car3", ref_line=3.4 / 2, v=round(80 / 3.6, 2), x=46 - 2 * distance2 - 5, y=3.4 / 2 + 3.4, v_=round(40 / 3.6, 2), h=0)
    # t.load_car(name="Car4", ref_line=3.4 / 2, v=round(80 / 3.6, 2), x=46 - 3 * distance2 - 5, y=3.4 / 2 + 3.4, v_=round(40 / 3.6, 2), h=0)
    # t.load_car(name="Car5", ref_line=3.4 / 2, v=round(80 / 3.6, 2), x=46 - 4 * distance2 - 5, y=3.4 / 2 + 3.4, v_=round(40 / 3.6, 2), h=0)


    # t.set_car_restrain("Car3", "Car2", distan)



    for i in range(100):
        print(i)
        t.run_loop()
    ani = Animation()
    for i in t.multi_car.keys():
        ani.load_trajectory(t.multi_car[i].historical_track)
        print(t.multi_car[i].historical_track)

    ani.draw()




